#include <LiquidCrystal.h>
#include <Ping.h>
#include <Servo.h>
################################################################################
# MIT License - Share/modify/etc, but please keep this notice.
#
# Copyright (c) 2011, Joey Barrett
# 
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
# 
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
# 
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
# THE SOFTWARE.
################################################################################

// Parallax Ping Unit for distance sensing
Ping sonic(22); 

// Setup LCD pins
LiquidCrystal lcd(7, 8, 9, 10, 11, 12);

// Servo for Ping unit sweeping
Servo  PingServo;

// Setup Ping distance setting variables
int pingDis;

// variable to keep commands in
char MsgRcvd;

// Motor Setup info

int LF[] = {23,24};
int LR[] = {25,26};
int RF[] = {27,28};
int RR[] = {29,30};

// Set Debugging here
// 1 - Debug on - Motors don't turn when commands are sent.
// 0 - Debug off - Motors turn when commands are sent.
int debug = 1;

//Variables for speed
int SpdPin = 22;
int Speed = 255;

void setup()
{
  Serial.begin(9600);  // start serial communications

// Setup Motors for output
  int i;
  for(i = 0; i < 2; i++){
    pinMode(LF[i], OUTPUT);
    pinMode(LR[i], OUTPUT);
    pinMode(RF[i], OUTPUT);
    pinMode(RR[i], OUTPUT);
  }
  
  // setup servo to sweep
     PingServo.attach(6);
   PingServo.write(90);
  
  // set up the LCD's number of rows and columns: 
  lcd.begin(16, 2);

  // Print a message to the LCD.
  lcd.print("Waiting...");

  // Setup Speed Pin
  pinMode(SpdPin, OUTPUT);
}

void loop()
{
  if (Serial.available() > 0)    //check to see if a command is available 
  {
    MsgRcvd = Serial.read();    // if command is there, see what it is.
    switch (MsgRcvd)
    {
    case '0':
      Stop();
      break;
    case '1':
      MoveForward();
      break;
    case '2':
      MoveLeft();
      break;
    case '3':
      MoveRight();
      break;
    case '4':
      MoveBackward();
      break;
    case 'I':
      active_ir();
      break;
    case 'M':                    // check to see if we have a connection from the gui - if so spit out info to the lcd.
      lcd.clear();
      lcd.print("Connected");
      lcd.setCursor(0,1);
      lcd.print("waiting..");
      break;
    case 'D':
      lcd.setCursor(0,1);
      lcd.print("Disconnected"); // client disconnected - spit out a disconnect to the lcd.
      break;
    }
  }
  delay(100);
}

//  ===================================
//  =====    Ping Ultrasonic      =====
//  ===================================
void active_ir()
{
  // Read to the Right
   PingServo.write(30);
  delay(300);
  pingDis = sonic.inch();
  delay(500);
  if (pingDis >= 12){
    Serial.println('2');
  }
  // Read to the Front
   PingServo.write(90);
  delay(300);
 pingDis = sonic.inch();
  delay(500);
  if (pingDis >= 12){
    Serial.println('1');
  }
  //  Read to the Left
   PingServo.write(150);
  delay(300);
  pingDis = sonic.inch();
  delay(500);
  if (pingDis >= 12){
    Serial.println('3');
  }
  return;
}



//  ==========================================
//  ======        MOTOR CONTROL      =========
//  ==========================================

void MoveForward()
{
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Forward");
  if (debug == 0){
 
    digitalWrite(LF[0], HIGH);
    digitalWrite(LF[1], LOW);
    digitalWrite(LR[0], HIGH);
    digitalWrite(LR[1], LOW);
    digitalWrite(RF[0], HIGH);
    digitalWrite(RF[1], LOW);
    digitalWrite(RR[0], HIGH);
    digitalWrite(RR[1], LOW);
  }
}

void MoveBackward()
{
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Reverse");
  if (debug == 0){
    analogWrite(SpdPin, Speed);
    digitalWrite(LF[0], LOW);
    digitalWrite(LF[1], HIGH);
    digitalWrite(LR[0], LOW);
    digitalWrite(LR[1], HIGH);
    digitalWrite(RF[0], LOW);
    digitalWrite(RF[1], HIGH);
    digitalWrite(RR[0], LOW);
    digitalWrite(RR[1], HIGH);
  }
}

void MoveLeft()
{
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Left");
  if (debug == 0){
    analogWrite(SpdPin, Speed);
    digitalWrite(LF[0], LOW);
    digitalWrite(LF[1], HIGH);
    digitalWrite(LR[0], LOW);
    digitalWrite(LR[1], HIGH);
    digitalWrite(RF[0], HIGH);
    digitalWrite(RF[1], LOW);
    digitalWrite(RR[0], HIGH);
    digitalWrite(RR[1], LOW);
  }
}

void MoveRight()
{
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Right");
  if (debug == 0) {
    analogWrite(SpdPin, Speed);
    digitalWrite(LF[0], HIGH);
    digitalWrite(LF[1], LOW);
    digitalWrite(LR[0], HIGH);
    digitalWrite(LR[1], LOW);
    digitalWrite(RF[0], LOW);
    digitalWrite(RF[1], HIGH);
    digitalWrite(RR[0], LOW);
    digitalWrite(RR[1], HIGH);
  }
}

void Stop()
{
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Stopping");
  if (debug == 0){
    digitalWrite(LF[0], LOW);
    digitalWrite(LF[1], LOW);
    digitalWrite(LR[0], LOW);
    digitalWrite(LR[1], LOW);
    digitalWrite(RF[0], LOW);
    digitalWrite(RF[1], LOW);
    digitalWrite(RR[0], LOW);
    digitalWrite(RR[1], LOW);
  }
}

